/*************************************************************************
 *
 *    Used with ICCARM and AARM.
 *
 *    (c) Copyright IAR Systems 2008
 *
 *    File name   : main.c
 *    Description : Define main module
 *
 *    History :
 *    1. Date        : 29, July 2008
 *       Author      : Stanimir Bonev
 *       Description : Create
 *
 *   This example project shows how to use the IAR Embedded Workbench
 *  for ARM to develop code for the IAR-STM32F103ZE-SK board.
 *   It implements USB CDC (Communication Device Class) device and install
 *  it like a Virtual COM port. UART1 is used for physical implementation
 *  of the RS232 port.
 *
 *  Jumpers:
 *   PWR_SEL - depending of power source
 *
 *    $Revision: 38757 $
 **************************************************************************/
#include "includes.h"
#include "utype.h"
#include "apptimer.h"
#include "AP_Baro_BMP085.h"
#include "mpu3050.h"
#include "I2CRoutines.h"
#include "MyRtos.h"
#include "modbus.h"
#include "pwm.h"
#include "adc.h"
#include "flash_pb.h"
#include "HMC58X3.h"
#include "hspo.h"
#include "math.h"
#include "WheelControl.h"

#define DLY_100US  450

#define UART  UART_1

Int32U CriticalSecCntr;

/*************************************************************************
 * Function Name: TickHandler
 * Parameters: void
 * Return: void
 *
 * Description:
 *		
 *************************************************************************/
void vApplicationTickHook(void)
{
  AppTimer(1);
}
/*************************************************************************
 * Function Name: SysTickInit
 * Parameters: none
 *
 * Return: none
 *
 * Description: Init SysTick
 *
 *************************************************************************/
#define         SYSTICK_1MS           (9000-1)
void SysTickInit(void)   // FreeRtos port.c also does the same thing
{
#if 0
  // Set SysTick source
  SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8); // 9 MHz Tick
  // Set Reload value
  SysTick_SetReload(SYSTICK_1MS);  // reload every 1 ms
  // Enable System Tick
  SysTick_CounterCmd(SysTick_Counter_Enable); // 9 MHz Tick 
  // Enable SysTick interrupt
  SysTick_ITConfig(ENABLE); 
#endif

  SysTick_Config(SYSTICK_1MS);  // 1ms
  SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);
  
}

void IoInit(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
  
  // Enable clock
  RCC_APB2PeriphResetCmd(  RCC_APB2Periph_AFIO  | RCC_APB2Periph_GPIOA
                         | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOD
                         | RCC_APB2Periph_GPIOC                            
                         | RCC_APB2Periph_GPIOE | RCC_APB2Periph_TIM1,
                           DISABLE);
  RCC_APB2PeriphClockCmd(  RCC_APB2Periph_AFIO  | RCC_APB2Periph_GPIOA
                         | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOD                         
                         | RCC_APB2Periph_GPIOC 
                         | RCC_APB2Periph_GPIOE | RCC_APB2Periph_TIM1,
                           ENABLE);
  
  // OD ouput, LED PB 3,4,
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4;
  GPIO_Init(GPIOB, &GPIO_InitStructure);  
  
  // OD output, Beeper PA12
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // debug output
  GPIO_Init(GPIOA, &GPIO_InitStructure); 
  
  GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
  GPIO_SetBits(GPIOB, GPIO_Pin_3); 
  I2C2_Init();
  Adc_Init();
}


/*************************************************************************
 * Function Name: SensorTask()
 * Parameters: none
 *
 * Return: none
 *
 * Description: main
 *
 *************************************************************************/
AP_Baro_BMP085  Baro;
MPU3050  Gyro;
static INT16U MPU3050Timer;
void SensorBaroTask( void *pvParameters )
{
  Baro.init();
  //Gyro.init(DEFAULT_MPU_SLAVEADDR);
  MPU3050Timer = SetTimer(0);
  while(1)
  {
     Baro.RefreshData();
#if 0
    if(ChkTimer(MPU3050Timer)==0){
      Gyro.readTemp((int*)(&Reg4xxxx[MPU3050_TEMP]));
      MPU3050Timer = SetTimer(1000);
    }
#endif    
    Yield();
  }  
}


void LedTask( void *pvParameters )
{
  for(;;){
    GPIO_WriteBit(GPIOB, GPIO_Pin_4,Bit_RESET);  
    Sleep(50);
    GPIO_WriteBit(GPIOB, GPIO_Pin_4,Bit_SET);  
    Sleep(950); 
  }
}

void PwmTask( void *pvParameters )
{
  int Width;
  
  Pwm_Init();
  Pwm_Output(0,0);
  Pwm_Output(1,1000);
  Pwm_Output(2,0);
  Pwm_Output(3,2000);
  Pwm_Output(4,2500);
  Pwm_Output(5,1800);
  
  do {
    Width = Pwm_Input(5);
    if(Width == -1){
      Pwm_Output(1,0);  // stop channel 1  
    }
    else Pwm_Output(1,Width); // copy channel 5 o/p to channel 1 
    if(Pwm_Period_Chk(5) == 0) Width = -1;
    Reg4xxxx[PWM_IN_REG] = Width;
    Yield();
  } while (1);
}

/*************************************************************************
 * Function Name: main
 * Parameters: none
 *
 * Return: none
 *
 * Description: main
 *
 *************************************************************************/
#include "parm.h"


PARM_DATA_BLOCK   ParmData;

void main(void)
{
  ENTR_CRT_SECTION();
  /* Setup STM32 system (clock, PLL and Flash configuration) */
  SystemInit();

  // NVIC init
#ifndef  EMB_FLASH
  /* Set the Vector Table base location at 0x20000000 */
  NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0);
#else  /* VECT_TAB_FLASH  */
  /* Set the Vector Table base location at 0x08000000 */
  NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
#endif
  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
  
//  SysTickInit();
  IoInit();
  
  EXT_CRT_SECTION();
   
  FlashPB_Init(256);  // 256 bytes parameter block include reserved word
  memcpy(&ParmData,FlashPBGet(),sizeof(ParmData));
  // Task creation
  //xTaskCreate( SensorTask, ( signed portCHAR * ) "Sensor", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
  xTaskCreate( LedTask, ( signed portCHAR * ) "Led", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
  //xTaskCreate( PwmTask, ( signed portCHAR * ) "Pwm", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
#ifdef CONFIG_ADC
  xTaskCreate( AdcTask, ( signed portCHAR * ) "Adc", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL ); 
#endif  
//  xTaskCreate( SensorBaroTask, ( signed portCHAR * ) "Baro", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
  //xTaskCreate( WheelSteerTask, ( signed portCHAR * ) "SteerWheel", configMINIMAL_STACK_SIZE+50, NULL, tskIDLE_PRIORITY, NULL );     
  xTaskCreate( ModbusTask, ( signed portCHAR * ) "Modbus", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
  //xTaskCreate( eCompassTask, ( signed portCHAR * ) "eCompass", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );

  //xTaskCreate( hspoTask, ( signed portCHAR * ) "hspo", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );   
  TaskRun();
}
